#include "cartographer/rcm/imu/common.h"

namespace cartographer
{
    int CollectImuByTimeSpan(const std::deque<sensor::ImuData> &imuSrc,
                             std::vector<sensor::ImuData> &imuSet,
                             const common::Time &time_start,
                             const common::Time &time_end,
                             const double timeOutThresh)
    {
        if (imuSrc.size() < 2)
            return -1;
        if (time_end <= time_start)
            return -2;
        for (auto iter = imuSrc.begin(); iter < imuSrc.end(); iter++)
        {
            if (iter->time < time_start)
            {
                const double delta = common::ToSeconds(time_start - iter->time);
                if (delta > timeOutThresh)
                {
                    continue;
                }
            }
            if (iter->time > time_end)
            {
                const double delta = common::ToSeconds(iter->time - time_end);
                if (delta > timeOutThresh)
                {
                    continue;
                }
            }
            if (HasNan(*iter))
                continue;
            imuSet.push_back(*iter);
        }
        int getSize = imuSet.size();
        if (getSize < 1)
            return -3;
        if (imuSet[0].time > time_start)
        {
            const double delta = common::ToSeconds(imuSet[0].time - time_start);
            if (delta > 0.1)
            {
                LOG(INFO) << "CollectImuByTimeSpan interval is too big for start time " << delta << "\n";
            }
        }
        if (imuSet[getSize - 1].time < time_end)
        {
            const double delta = common::ToSeconds(time_end - imuSet[getSize - 1].time);
            if (delta > 0.1)
            {
                LOG(INFO) << "CollectImuByTimeSpan interval is too big for end time " << delta << "\n";
            }
        }
        return 1;
    }
} // namespace cartographer